package de.ostfalia.robotik.rob3;
import gnu.io.NoSuchPortException;
import gnu.io.PortInUseException;


public class Rob3 {
	private static final int TIMEOUT = 500;
	private static final double[] MIN_ANGLES = { -80, -68,   0,  -82, -100,   0};
	private static final double[] MAX_ANGLES = { +80, +32, 100, +118, +100,   1};
	private static final int[] CENTER_VAL = {127, 174, 0, 105, 127, 0};
	
	public enum Axis {
		Base(0),
		Shoulder(1),
		Elbow(2),
		Tool_Pitch(3),
		Tool_Roll(4),
		Gripper(5);
		
		private int mAxisNr;
		private Axis(int axisNr) { mAxisNr = axisNr; }
		public int getAxisNr() { return mAxisNr; }
	}
	
	public class RobotNotAnsweringException extends Exception {
		private static final long serialVersionUID = 6596749489487567035L;
	}
	
	private Comm mComm;
	
	public Rob3(String comPortName) throws NoSuchPortException, PortInUseException, RobotNotAnsweringException {
		mComm = new Comm(comPortName);
		
		mComm.send(0x20);
		int[] acks = { 0x15, 0xF1 };
		if (mComm.waitForChar(acks, TIMEOUT) < 0) {
			throw new RobotNotAnsweringException();
		}
	}

	public void setAxis(Axis axis, int value) {
		if (value < 0) { value = 0; }
		if (value > 255) { value = 255; }
		
		byte[] data = new byte[3];
		data[0] = (byte)(axis.getAxisNr() + 8); // 8 = With ack
		data[1] = (byte)value;
		data[2] = 0x03;
		mComm.send(data);
	}
	
	public void setAxisAngle(Axis axis, double degree) {
		int value = 0;
		int axisNr = axis.getAxisNr();

		value = (int)Math.round(((255.0 / (MAX_ANGLES[axisNr] - MIN_ANGLES[axisNr])) * degree)) + CENTER_VAL[axisNr];
		
		System.out.println("setAxisAngles: axis="+axis.getAxisNr()+", deg="+degree+", val="+value);
		
		setAxis(axis, value);
	}
	
	public int getAxis(Axis axis) {
		byte[] data = new byte[2];
		byte cmd = (byte)(0x40 + axis.getAxisNr());
		data[0] = cmd;
		data[1] = 0x03;
		mComm.send(data);

		data = mComm.receiveMessage(0x03, TIMEOUT);
		if (data != null) {
			int offset = 0;
			do {
  			if (data[offset + 0] == cmd) {
  				int val = data[offset + 1];
  				if (val < 0) {
  					val = 256 + val;
  				}
  				return val;
  			}
  			while (data[offset] != 0x03) {
  				offset++;
  			}
  			offset++;
			} while (offset < data.length);
		}
		return 0;
	}
	
	public double getAxisAngle(Axis axis) {
		int value = getAxis(axis);
		int axisNr = axis.getAxisNr();
		
		return (((MAX_ANGLES[axisNr] - MIN_ANGLES[axisNr]) / 255.0) * (value - CENTER_VAL[axisNr]));
	}
	
	public static double getMinAngle(Axis axis) {
		return MIN_ANGLES[axis.getAxisNr()];
	}

	public static double getMaxAngle(Axis axis) {
		return MAX_ANGLES[axis.getAxisNr()];
	}
}
